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ABSTRACT 

In  recent  years,  pseudospectral  (PS)  methods  have  been  successfully  applied  to  solve  a  rich  number  of 
trajectory  optimization  problems  arising  in  aerospace  application.  Motivated  by  the  success  of  this  approach, 
we  consider  the  problem  of  generating  minimum-time  trajectories  for  unmanned  ground  vehicles.  Shapes  of 
arbitrary  number,  size  and  configuration  are  modelled  in  the  form  of  path  constraints  in  the  resulting  optimal 
control  problem  formulation.  PS  methods  are  used  to  solve  the  constrained,  nonlinear  optimal  control 
problem.  Solutions  are  obtained  within  a  few  seconds  even  under  a  MATLAB  environment  running  on  legacy 
computer  hardware.  A  complete  problem  formulation  is  developed  including  a  derivation  of  the  necessary 
conditions  for  optimality.  The  trajectories  are  mathematical  extremals  as  they  satisfy  all  the  optimality 
conditions.  What  is  noteworthy  about  our  approach  is  that  the  difficulties  associated  in  solving  the 
Hamiltonian  equations  are  completely  circumvented  by  an  application  of  the  Covector  Mapping  Theorem. 
Solved  examples  are  provide  to  illustrate  the  tools  and  techniques. 


1.0  INTRODUCTION 

Trajectory  planning  occurs  within  the  guidance  system  of  every  autonomous  or  semi-autonomous  vehicle, 
whether  it  is  the  $300  Roomba®  or  the  $300M  New  Horizons  spacecraft.  Trajectory  planning  allows  the 
vehicle  to  travel  from  one  location  to  another  safely,  within  the  limits  of  its  electrical  and  mechanical 
capabilities,  and  in  some  situations,  in  an  optimal  manner  with  respect  to  use  of  fuel,  expenditure  of  time,  or 
distance  travelled.  The  development  of  autonomous  trajectory  planning  technologies  has  been  painstakingly 
slow;  the  variety  of  tasks  accomplished  by  every  five-year-old  human  cannot,  at  this  point  in  time,  be 
replicated  by  a  multi-million  dollar  robot.  The  advantages,  however,  to  accomplishing  human-like  tasking 
with  a  robot  are  immeasurable  and  for  that  reason  continue  to  be  a  goal  of  researchers  everywhere. 

1.1  Motivation 

A  broad  class  of  problems  are  emerging  that  implore  the  ability  to  remove  the  human  from  the  control  loop. 
The  rewards  of  robotic  systems  are  increased  production,  reliability,  safety,  and  capability.  Spacecraft 
assembly  and  repair,  space  station  replenishment,  and  certain  facets  of  planetary  exploration  were  viewed 
solely  as  human  missions  during  the  Apollo  and  early  Space  Shuttle  periods.  Ever  improving  technologies 
and  capabilities  and  cost  and  risk  reduction  have  been  the  driving  forces  in  reshaping  these  paradigms;  now, 
systems  are  either  being  developed  or  have  already  been  fielded  to  accomplish  these  tasks  unmanned.  The 
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Mars  rovers  Spirit  and  Opportunity  are  shining  examples  of  the  utilization  of  robotic  technologies  to  aid 
human  exploration,  accomplishing  tasks  that  could  not  be  achieved  otherwise.  The  applications  of  trajectory 
planning  are  not  limited  to  systems  outside  the  Earth’s  atmosphere.  Military  applications  of  unmanned  and 
autonomous  vehicles  have  drawn  considerable  interest  and  recognition  in  recent  years.  Most  notable  is  the 
Unmanned  Aerial  Vehicle  (UAV),  but  considerable  advances  are  making  the  incorporation  of  unmanned 
ground  (UGV)  and  sea  surface  vehicles  possible.  Meanwhile,  new  classes  of  UAVs  are  under  development 
that  are  carried  by  soldiers  on  the  ground,  deployed  only  when  operationally  needed,  and  implemented 
without  the  overhead  of  the  larger  current  systems.  These  systems  remove  humans  from  jobs  that  would  be 
otherwise  extremely  dangerous.  Moving  away  from  the  public  sector,  the  private  arena  is  the  ultimate  stage 
for  technological  utility.  While  the  future  possibilities  are  truly  endless,  robotic  technologies  are  already 
improving  consumer  lives  by  eliminating  such  tedious  tasks  as  vacuuming  and  parallel  parking. 

1.2  Background 

The  various  applications  of  trajectory  planning  led  to  the  evolution  of  differing  techniques  to  solve  their 
problems.  Aeronautical  and  space  applications  drove  the  development  of  nonlinear  optimal  control 
techniques  and  continue  to  do  so  [1-6].  In  these  situations,  global  problem  knowledge  is  assumed,  vehicle 
motion  is  precisely  understood,  few  physical  constraints  obstruct  the  vehicular  motion,  and  trajectory 
optimization  is  key  to  design  and  mission  success.  Robotics  applications  propelled  a  differing  solution,  but  it 
was  a  solution  better  adapted  to  fit  the  inherent  situational  complexities  including:  limited  problem 
knowledge,  noisy  sensors,  uncertain  dynamical  characteristics,  intricate  obstacle-rich  environments,  limited 
computational  power,  and  the  necessity  to  generate  feasible  solutions.  Despite  the  general  similarity  between 
problems,  these  two  fields  of  application  experienced  little  cross-pollinization.  Aerospace  applications 
desired  optimality  and  robotics  applications  desired  simplicity  and  an  ability  to  handle  uncertainty. 

Many  techniques  fall  within  the  realm  of  robotics-based  trajectory  planning  [7-8].  Potential  functions  were 
first  developed  over  thirty  years  ago  [9],  and  though  they  can  be  simplistic,  they  are  still  used  in  many 
functions  today  [10-11].  In  general,  this  approach  can  be  used  successfully  in  the  presence  of  uncertainty  but 
it  suffers  from  occurrences  of  local  minima,  a  lack  of  trajectory  optimality,  and  difficulty  in  accommodating 
complex  vehicle  constraints  [12].  More  recent  developments  place  heavy  emphasis  on  sampling-based 
planning  techniques.  Notably,  sampling  methods  include  Probabilistic  Road  Maps  (PRM),  Rapidly-Exploring 
Random  Trees  (RRT),  and  Expansive  Space  Trees  (EST)  to  name  a  few  [8,  13-15].  On  the  whole,  these 
methods  use  a  probabilistic  means  of  connecting  the  initial  configuration  to  the  final  configuration,  enabling 
an  improved  capacity  to  handle  uncertainty,  a  relative  blindness  to  environmental  complexity,  and  a  capability 
to  generate  feasible  solutions  with  minimal  computational  burden.  Complexity  increases  with  the 
incorporation  of  planning  techniques  to  satisfy  dynamic  constraints  and  improve  solution  optimality;  however, 
solution  optimality  is  never  guaranteed  [15]. 

Nonlinear  optimal  control  methods  offer  significant  advantages  to  other  approaches,  namely  the  inherent 
satisfaction  of  vehicle  and  problem  constraints  and  the  intrinsic  extremal  nature  of  solutions.  Generally,  two 
distinct  methods  exist  for  solving  the  nonlinear  optimal  control  problems  [16-17],  direct  and  indirect.  Indirect 
methods  make  use  of  Pontryagin’s  maximum  principle  and  create  a  two-point  boundary  value  problem. 
Direct  methods  transfer  the  problem  into  a  nonlinear  programming  problem  by  discretizing  the  trajectory  and 
this  problem  can  be  solved  by  any  one  of  a  number  of  nonlinear  optimization  solvers.  Typically  direct 
methods  are  considered  more  numerically  advantageous  that  indirect  methods,  but  despite  their  differences, 
they  all  share  the  same  concerns  including:  solution  convergence,  computational  complexity,  and  the  need  for 
a  good  initial  guess.  Advances  in  computing  power  and  improvement  in  optimization  algorithms  are  changing 
that  paradigm  and  research  is  showing  the  applicability  of  optimal  control  techniques  in  guidance  and  control 
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[18-21].  Whereas  optimization  methods  traditionally  created  a  solution  in  minutes  to  hours,  solutions  can  now 
be  found  within  seconds  or  less,  and  the  improvements  in  computational  speed  increase  their  ability  to  handle 
problem  uncertainty.  Aerospace  applications  made  use  of  optimal  control  solutions  because  the  problems 
typically  permitted  the  trajectories  to  be  generated  offline;  now  the  improvements  in  optimal  control  methods 
present  a  strong  argument  for  their  application  in  online  planners  and  with  robotics  problems. 


2.0  SOLUTION  APPROACH 

Given  the  progress  made  in  applying  optimal  control  methods  to  online  aerospace  applications  [18-19]  and 
being  aware  of  the  ability  to  eliminate  the  associated  computational  burden  [20,  22],  this  paper  focuses  on  the 
application  of  these  techniques  to  a  new  breed  of  problems,  sharing  attributes  with  both  aerospace  and 
robotics  applications.  This  problem  is  that  of  the  unmanned  vehicle,  characterized  by  a  lack  of  global 
knowledge,  complex  obstacle-rich  environments,  and  a  need  for  feasible  solutions  in  the  face  of  uncertainty. 
On  the  other  hand,  these  systems  typically  incorporate  sophisticated  sensors  and  ample  computational  power. 

The  research  presented  here  evaluates  and  validates  the  concept  of  trajectory  planning  for  unmanned  vehicles 
with  optimal  control  methods.  Being  a  feasibility  study,  robustness  is  not  proven,  and  while  the  concerns  of 
uncertainty  are  currently  being  evaluated,  those  efforts  are  not  discussed  here.  Instead,  the  primary  thrust  of 
this  work  is  the  optimization  of  kinematic  trajectories  within  varying,  complex  environments.  The 
optimization  tool  used  during  this  exercise  was  DIDO®,  a  software  package  based  on  pseudospectral  methods 
[1-6]  that  runs  within  the  MATLAB  environment. 


3.0  THE  OPTIMAL  CONTROL  PROBLEM 

One  of  the  many  advantages  to  utilizing  optimal  control  methods  is  the  relative  ease  with  which  a  multitude  of 
problems  is  formulated.  Any  kinematic,  dynamic,  or  path  constraint  or  optimality  criteria  that  is  appropriately 
expressed  in  a  mathematical  manner  can  be  rapidly  incorporated  into  the  problem  formulation.  This  inherent 
portability  and  flexibility  makes  these  techniques  powerful,  but  the  fundamental  key  is  to  express  the 
characteristics  appropriately.  In  this  section,  the  UGV  problem  will  be  defined  within  the  framework  of  an 
optimal  control  problem;  all  characteristics  will  be  covered  including  the  kinematic  equations,  path 
constraints,  scaling,  and  necessary  conditions.  Even  though  the  detailed  focus  remains  on  one  version  of  an 
UGV,  the  specific  equations  are  not  as  important  as  the  validation  of  the  method;  due  to  generality  of  this 
method  any  system  could  be  represented. 

3.1  Unmanned  Ground  Vehicle 

The  term  “unmanned  ground  vehicle”  is  generic;  there  are  a  multitude  of  systems  that  could  fit  that  name.  In 
an  attempt  to  maintain  generality,  a  common  example  is  used  -  a  four-wheeled  car  with  rear-wheel  drive  and 
front-wheel  steering.  This  system  is  well-studied,  and  the  nonholonomic-nature  of  the  constraints  adds 
kinematic  complexity.  A  simple,  kinematic  model  of  the  car  [8]  is  shown  in  Figure  1  and  the  state  and  control 
vectors  are  presented  in  Equations  (1)  and  (2),  respectively.  The  x-y  location  of  the  car  represents  the  current 
position  of  the  center  point  of  the  rear  axle.  The  car’s  orientation  angle  is  measured  with  respect  to  the 
horizontal  axis  and  is  presented  as  the  state  variable  theta.  The  steering  angle,  phi,  is  measured  with  respect  to 
the  car’s  heading,  or  velocity  vector,  and  the  variable  ‘L’  measures  the  distance  between  the  front  and  rear 
axles. 
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Figure  1:  A  four-wheel  car  model  with  front-wheeled  steering. 


x  = 


(i) 


u  = 


where  u  e  U  =  • 


fv:  vmin<v(0<vm 
\<f>-  t)<(j)a 


(2) 


The  steering  angle  must  be  between  ±90°  for  computational  and  for  practical  purposes  it  is  typically  much  less 
than  that.  Unlike  the  Reeds-Shepp  or  Dubin’s  cars  the  velocity  is  allowed  to  change  continuously  between  the 
extremes  similarly  to  the  capability  of  a  real  car. 

Given  this  UGV  characterization,  Equation  (3)  shows  the  resulting  nonlinear,  kinematic  equations  and 
Equation  (4)  provides  the  vehicle’s  current  turning  radius. 


x  = 


vcos# 

vsin# 

v  . 

—  tan$> 


r  — 

f,mm 


tan  <t>„ 


(3) 


(4) 
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3.2  Obstacle  Representation 

In  general,  a  constraint  can  be  a  function  of  the  state,  control,  or  time  so  long  as  it  is  written  mathematically. 
For  the  car  problem,  there  exist  vehicle  constraints  -  holonomic  kinematics  and  control  constraints  -  and  path 
constraints.  The  path  constraints  are  the  obstacles  around  which  the  car  must  maneuver.  For  simplicity  and 
computational  efficiency  it  was  desirable  to  represent  the  path  constraints  as  continuous,  differentiable 
algebraic  functions.  With  this  framework  in  mind,  the  p-norm  was  used  to  create  generic  shapes  including: 
diamonds,  circles  and  ellipses,  and  squares  and  rectangles.  Figure  2  demonstrates  how  the  p-norm  can  be 
manipulated  to  create  these  obstacle  shapes;  the  ellipse  and  rectangle  are  simply  extensions  of  the  circle  and 
square  respectively  where  the  distance  along  the  x  and  y  axes  are  dissimilar.  Equation  (5)  presents  the 
common  equation  used  to  build  these  shapes.  Any  point  outside  the  obstacles  yields  a  path  constraint  value 
greater  than  zero.  The  square  and  rectangular  shapes  result  from  the  infinity  norm;  however,  during 
calculations,  this  is  approximated  by  a  large  value  for  the  exponent.  Higher  values  for  the  exponent  yielded 
sharper  comers,  but  they  often  created  numerical  concerns.  The  square  in  Figure  2  was  created  by  setting  the 
exponent  to  100. 

2 
1 
0 
-1 
-2 

-2  0  2  -2  0  2  -2  0  2 

Figure  2:  Unit  p-norms  for  p  =  1,  2,  and  infinity  respectively. 


'x(t)-xc'P 


+ 


—  c 


(5) 


Modelling  shapes  outside  of  these  basic  geometries  is  a  powerful  option  and,  in  most  cases,  more  realistic  of 
the  random  nature  of  the  world.  However,  even  simple  polygonal  shapes  are  non-trivial  since  they  cannot  be 
wholly  described  by  one  algebraic  equation.  Figure  3  is  a  straightforward  example  of  a  simple  shape, 
described  by  the  confluence  of  three  lines,  which  cannot  be  characterized  by  the  p-norm. 


Figure  3:  Simple  polygonal  constructed  by  three  lines. 
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Using  the  equation  for  each  line  it  is  possible  to  create  a  series  of  inequality  constraints  that  defines  each  point 
within  the  obstacle.  This  concept  is  evidenced  by  Equation  Set  (6). 


2  U) 

~y  +  x  +  3 

l(*)  = 

&2  U) 

_<?3  (*)_ 

— 

y  +  -  4 

y  -4x  + 15 

(6) 


While  every  point  within  the  obstacle  satisfies  all  of  the  three  constraints,  every  point  outside  the  obstacle 
must  not  satisfy  at  least  one;  this  is  a  mathematical  4 or’  condition.  Given  that  continuous  and  smooth 
functions  are  desirable  for  DIDO  implementation,  a  simple  logical  operation  could  not  be  implemented. 
Instead,  the  resulting  g  vector  must  be  searched  for  the  minimum  single  value;  if  this  value  is  less  than  zero,  it 
can  be  stated  that  the  point  must  lie  outside  the  obstacle.  From  a  mathematical  perspective,  every  feasible 
point  on  the  map  in  Figure  3  satisfies  Equation  (7). 

^  (x(f),  j(0)  =  minjg!  (x),g2  (x),g3  (*)}  <  0  (7) 


Figure  4  illustrates  this  concept.  The  left  panel  shows  the  mathematical  constraints  of  the  triangular  obstacle 
and  right  panel  shows  the  resulting  values  of  the  new  path  constraint  function  over  the  positional  domain. 


X-axis 


Figure  4:  Non-smooth  constraint  representation  of  a  simple  polygonal  shape. 


The  sharp  corners  are  captured  exactly  with  this  technique.  The  result  is  continuous  and  grows  linearly  with 
distance  from  the  obstacle;  however,  the  result  is  not  smooth,  i.e.  non-differentiable.  The  non-smoothness 
arrives  from  the  fact  that  the  path  constraint  is  essentially  the  union  of  three  planes.  It  is  possible  to  correct 
the  non-smoothness,  but  this  comes  at  a  loss  of  accuracy  in  rendering  the  sharp  comers.  Equation  (8)  displays 
the  mathematical  expression  for  the  smooth  polygonal  constraint,  and  it  occurs  in  three  distinct  steps. 


hPi(x(t),y(t))  =  \n 


(e-glU)-P  +  e~S2  U)-P 


+  e 


r&  (*)•/> 


A/p 


>0 


(8) 


In  the  first  step,  the  elements  of  g  are  multiplied  by  negative  one  and  placed  into  an  exponent.  This  maps  all 
previously  positive  values  of  g  to  numbers  between  zero  and  one  and  all  previously  negative  values  to 


10-6 


RTO-MP-AVT -1 46 


UNCLASSIFIED/UNLIMITED 


UNCLASSIFIED/UNLIMITED 

A  Pseudospectral  Method  for  Real-Time 
Motion  Planning  and  Obstacle  Avoidance 


numbers  greater  than  one.  The  next  step  is  to  take  a  p-norm;  this  step  extracts  the  largest,  single  value  of  g. 
The  last  step  uses  the  natural  logarithm  to  scale  the  values  back  to  the  original  range.  In  this  case  a  positive 
value  for  the  path  constraint  indicates  a  position  outside  the  obstacle,  illustrated  in  Table  1.  Both  the  power  of 
the  p-norm  and  the  base  of  the  exponent/logarithm  can  be  altered  to  provide  better  smoothness.  Figure  5 
provides  a  picture  of  the  result;  the  loss  of  sharpness  around  the  comers  is  visible.  Without  a  precise 
representation  of  the  obstacle’s  corners,  it  is  necessary  during  implementation  to  add  a  small  buffer  around 
obstacles  to  ensure  that  the  path  produced  is  actually  obstmction-free. 


Table  1:  Mapping  of  vehicle  position  to  path  constraint  value. 


Inside  Obstacle 

On  Obstacle 

Outside  Obstacle 

g(x) 

>0 

=  0 

<0 

■g(x) 

<0 

=  0 

>0 

eg® 

[0,1] 

=  1 

>  1 

II  es(xJ||p 

[0,1] 

=  1 

>  1 

ln(||  e  g®|p) 

<0 

=  0 

>0 

X-axis 


Figure  5:  Smoothed  constraint  representation  of  a  polygonal  shape. 

The  p-norm  and  polygonal  representations  of  obstacles  can  be  used  interchangeably  or  simultaneously  during 
problem  formulation.  While  the  polygonal  representation  is  more  functional,  it  is  slightly  more  complex  to 
prepare. 

3.3  Problem  Formulation 

After  presenting  the  kinematic  and  path  constraints,  the  optimal  control  problem  can  be  stated  mathematically. 
Conceptually,  the  problem  is  defined  as  the  generation  of  a  time-optimal  trajectory  that  enables  the  UGV  to 
travel  from  some  initial  condition  to  some  final  condition  while  satisfying  the  kinematic  and  path  constraints. 
Before  proceeding,  it  is  important  to  remember  that  the  optimal  control  problem  is  solved  with  a  numerical 
optimization  tool  and  for  that  reason  good  scaling  is  necessary  [23].  With  this  in  mind,  Equation  Set  (9) 
shows  the  new  scaled  variables  that  will  be  used  during  the  problem  formulation  -  the  scaling  factor  for  each 
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variable  is  represented  as  the  capitalized  version  of  that  variable  letter  and  the  scaled  variable  is  denoted  by 
the  bar. 


x 


x  = 


7 

0 


(9) 


If  the  scaling  factors  for  the  control  variables  are  set  to  the  maximum  respective  value,  the  optimal  control 
problem  can  be  represented  mathematically  as  Equation  Set  1 0. 


x  = 


eX  =R\  ueU  :=<- 


v  :  ~l<v  <1 


<f>\~\<0<  11 


Minimize  J [x  (•) ,  u  (•) ,  tf  ]  =  tf 


Subject  to 


x 


V  -  cos 

v  -  sin 
v 


(06») 

(@0) 


t,\V-T 


X 
7,\V  T 


v  <^-r\V-T 

— -tan \$>(b  - 

L  K  ’  ® 


:(0  =  [o] 


^o=0 


rf/i 

iTf)- 


■yj 


=  [o] 


hi(x(t),y(t))>Q 


(10) 


In  this  formulation,  the  initial  condition  includes  the  position  and  orientation  while  the  endpoint  condition 
includes  only  the  position.  If  the  final  orientation  of  the  vehicle  is  important,  it  could  be  included  with  little 
effort. 


3.4  Optimality  Conditions 

Proving  that  the  results  of  an  optimal  control  problem  are  indeed  globally  optimal  is  intractable,  but  several 
criteria  can  be  demonstrated  to  evidence  the  extremal  nature  of  the  solution.  The  criteria  used  within  this 
research  included  satisfaction  of  the  necessary  conditions,  verification  of  solution  feasibility,  and  validation  of 
Bellman’s  principle.  Bellman’s  principle  was  validated  by  ensuring  the  concurrence  of  optimal  sub¬ 
trajectories  that  begin  and  end  on  the  originally  defined  solution.  Feasibility  is  verified  by  propagating  the 
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system  kinematics  from  the  initial  condition  using  the  calculated  optimal  control  trajectory  and  MATLAB’s 
Runge-Kutta  algorithm.  The  necessary  conditions  for  optimality,  presented  below,  were  verified,  where 
possible,  against  the  solution  received  from  DIDO. 

For  the  scaled  problem  introduced  in  the  previous  subsection,  the  control  Hamiltonian  can  be  written  as 
Equation  Set  (11). 


H^X,x,u,  t  )  =  X1 


v  •  cos  (©0 

^V-T 


v  -  sin 


v 

—  •tan 
L 


(G0) 
(O 


Y 

V-T 

~G~ 


(11) 


where  XT  =^Xx  Xy  XffJ 

Using  the  Hamiltonian,  the  optimal  control  problem  can  be  reformulated  and  presented  as  Equation  (12). 


Minimizeu  H  (X,x,u,t^j 
Subject  to  ueU 

hi(x(T),y(T))>0 


(12) 


Equation  (13)  displays  the  resulting  Lagrangian  of  the  Hamiltonian. 


H^/u,X,x,u,t )  =  X 


V  -  cos 


(06>) 


v  •  sin 


X 

-\V-T_ 
Y 

-xF-77 


m- 

V  /, 

—  •  tan  0 
L  v 

where  JuT  =^Juv  j 


0 

Mh\ 


— T 

+  M 


v 

* 


h(x(t),y(t)) 


(13) 


After  creating  the  mathematical  problem,  it  is  possible  to  formulate  the  necessary  conditions  for  optimality. 
The  first  provision  is  the  Hamiltonian  Minimization  Condition  (HMC)  and  its  affect  is  shown  Equation  (14). 


8H 

du 


X  •  cos 


(00) 


^x  T-V  - 


■  +  X  -sin 
X  y 


Mt'v  t 


Y 


+  Xe  -tan(0 


-x  T-V  _ 

T~c^  +  Mv 
L  -  0 


tx  r-0  _ 


XH  -v  -  sec2  (0<z i) - h 

6  V  ’  L-& 


(14) 
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Complementing  this  result  is  the  Karush-Kuhn-Tucker  (KKT)  conditions  for  the  control  and  path  covectors. 


Mv 


Mu 


<0 

=  0  for 

>0 

<0 

=  0  for 

>0 

=  0 
<0 


for 


v(d=-i 

\<v[t  )<1 

vp)  =  l 

~\<(/)(T)<\ 

f  (7)  =  1 

hi(x(T),y(T))>0 
hi(x(T),y{f))  =  0 


(15) 


Given  the  time  optimality  of  the  trajectory,  it  is  expected  that  Equation  (16)  be  true. 

Juv  >  0  and  =  0  (16) 

The  first  condition  corresponds  to  a  maximum  vehicle  velocity,  and  the  second  corresponds  to  steering  angle 
between  the  extremes.  Using  these  predictions  little  can  be  said  for  the  first  equation  of  the  HMC;  however, 
the  first  term  in  the  second  equation  must  be  zero  to  satisfy  optimality  conditions  when  non-extremal  steering 
values  are  indicated.  This  can  only  occur  if  Equation  (17)  is  true  when  the  car  is  in  motion. 

\=0  (17) 


The  Adjoint  Equation,  Equation  (18),  describes  the  change  of  the  costates  over  time. 


K 

K 

Xg 


dH 

dx_ 


dh 


dh. 


i+ 1 


/4,/  '  +  /4,;+i 

ox  ox 


dh 


dh. 


i+\ 


f^. h,i  ' 

dy  8y 


+  ... 


+  ... 


Ax  -v-sin^©^) 


r-v-e 


+  Ay  -v-cos^©^) 

X  y 


rv@ 


(18) 


From  this  equation  and  the  KKT  conditions,  it  can  be  deduced  that  Ax  and  X  will  remain  constant  whenever 
the  vehicle  is  not  touching  an  obstacle. 

The  Terminal  Trans versality  Condition  (TTC),  Equation  (19),  provides  boundary  condition  information  on  the 
costates.  In  this  equation,  E  (c,  ,  tf  )  is  the  Endpoint  Lagrangian  and  E  ( ,  tf  )  is  the  Endpoint  Cost. 

Applying  this  condition  to  the  car  problem  yields  Equation  (20),  and  it  reveals  the  fact  that  Equation  (17)  must 
be  true  at  the  final  time. 
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dE 

dxf 


where  E{v_ ,  xf,  tf  )  =  E  (xf,tf  )  +  vje[xf,tf\ 
and  vT  =[vx  vy  Vg] 


V,Xf,tf 


)-tf+VT 


x{tf)-Xf 

y(Tf)-yf 


l-f 

1 _ 

V 

)= 

A'  f/) 

= 

Vy 

_0_ 

(19) 


(20) 


The  final  two  necessary  conditions  pertain  to  the  value  of  the  Hamiltonian.  The  first  is  the 
Hamiltonian  Value  Condition  (HVC),  Equation  (21),  which  is  applied  in  situations,  such  as  this,  where  the 
final  time  is  not  fixed. 


hM+§=°  =>  "('/)='>  <2» 

This  result  is  complemented  by  the  Hamiltonian  Evolution  Equation  (HEE),  Equation  (22),  which  describes 
the  time-evolution  of  the  Hamiltonian  value. 


H  = -  =>  H  =  0  (22) 

St 

Together  these  two  outcomes  show  that  the  value  of  the  Hamiltonian  must  be  a  constant  negative  one 
throughout  the  trajectory. 

In  summary,  these  aforementioned  conditions,  alongside  the  feasibility  verification  and  Bellman’s  principle 
validation,  cannot  prove  global  optimality,  but  they  provide  a  demonstration  of  the  extremal  nature  of  the 
DIDO  generated  solution.  In  the  very  least,  the  existence  of  a  feasible  solution  alone  permits  maneuver 
completion,  a  necessity  for  the  unmanned  vehicle  application. 


4.0  RESULTS 

The  results  of  this  work  are  presented  in  two  subsections.  First,  a  basic  problem  is  presented  such  that  the 
optimality  conditions  can  be  analyzed.  The  more  intricate  problems  that  follow  include  only  results.  The 
entire  data  is  not  included  only  for  the  purposes  of  brevity. 

4.1  Meeting  the  Optimality  Conditions 

A  simple,  three-circle  problem  was  evaluated  for  the  purposes  of  displaying  the  feasibility  and  optimality  of 
this  solution  approach.  In  the  X-Y  coordinate  frame,  the  vehicle  is  intended  to  travel  from  the  location  (0,  0) 
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and  oriented  down  the  X-axis  to  the  location  (10,  10).  The  optimal,  vehicle  state  trajectory,  as  created  in 
DIDO,  is  shown  in  Figure  6  and  Figure  7.  There  is  a  noticeable  distance  between  the  vehicle  trajectory  and 
the  side  of  the  obstacle;  the  reason  for  this  separation  is  in  accounting  for  the  size  of  the  UGV. 


Figure  6:  Time-optimal  UGV  trajectory  through  three  circular  obstacles. 


Figure  7:  UGV  orientation  during  time-optimal  trajectory. 
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The  UGV  trajectory  is  straight  unless  it  is  maneuvering  to  avoid  an  obstacle,  thus  displaying  the  optimal 
nature  of  the  result.  The  optimal  control  trajectory  that  generated  this  state  path  is  shown  in  Figure  8.  Using 
this  control  trajectory  to  test  the  feasibility  of  the  solution,  Figure  9  shows  the  comparison  of  the  propagated 
and  DIDO-generated  states.  The  propagated  and  DIDO-generated  states  are  identical;  this  indicates  solution 
feasibility. 


-40 

0  20  40  60 

Time  (sec) 


Figure  8:  UGV  time-optimal,  control  trajectory. 


Figure  9:  Feasibility  verification  of  time  optimal  control  trajectory. 
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The  next  several  figures  present  elements  of  the  necessary  conditions.  Figure  10  displays  the  value  of  the 
Hamiltonian  throughout  the  trajectory;  this  result  verifies  Equation  (21)  and  Equation  (22).  Figure  1 1  displays 
the  confirmation  of  Equation  (14),  the  Hamiltonian  Minimization  Condition,  where  the  partial  derivative  of 
the  Hamiltonian  with  respect  to  each  control  variable  should  be  equal  to  zero.  The  results  display  some 
numerical  error,  but  this  is  minimal  and  does  not  distract  from  the  feasibility  of  the  solution. 
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Figure  10:  Evolution  of  Hamiltonian  value  during  time-optimal  trajectory. 
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Figure  11:  Verification  of  Hamiltonian  Minimization  Condition. 


Figure  12  presents  the  costates  and  shows  that  Equation  (17)  is  true,  i.e.  A0  equals  zero.  Figure  13  shows  the 
path  constraint  covectors,  and  in  accordance  with  the  KKT  conditions,  the  covectors  are  only  active  while  the 
vehicle  is  touching  a  path  constraint.  Analyzing  Figure  13  alongside  Figure  12,  it  is  apparent  that  Equation 
(18)  is  proven;  namely,  Ax  and  Ay  are  constant  unless  the  vehicle  is  touching  an  obstacle. 
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Figure  12:  Costate  values  during  time-optimal  trajectory. 


Figure  13:  Path  constraint  covector  values  during  time-optimal  trajectory. 

The  last  necessary  condition  is  the  KKT  condition  associated  with  the  control  variables.  The  maximum 
velocity  is  used  throughout  the  trajectory  and  for  that  reason  it  maintains  a  positive  value.  Similarly,  a  non¬ 
maximum  value  of  the  steering  angle  is  used  and  it  maintains  a  value  of  zero. 
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Figure  14:  Control  constraint  covector  values  during  time-optimal  trajectory. 


The  remaining  condition  of  optimality  is  Bellman’s  principle.  In  this  experiment,  Bellman’s  principle  is 
validated  by  comparing  three  optimal  sub-trajectories  to  the  original  optimal  solution.  These  sub-trajectories 
begin  along  the  original  trajectory  at  the  one-third,  one-half,  and  two-thirds  distance  locations  as  shown  in 
Figure  15.  Figure  15  illustrates  the  fact  that  all  three  sub-trajectories  are  coincident  with  the  original  time- 
optimal  trajectory  and  Figure  16  illustrates  the  fact  that  the  maneuver  cost  is  consistent  across  each  trajectory. 
This  provides  further  evidence  to  the  extremal  nature  of  the  DIDO-generated  solution. 

In  summary,  the  results  shown  above  display  the  agreement  of  the  DIDO  generated  solution  with  the 
conditions  of  optimality.  The  global  optimality  of  the  solution  is  not  guaranteed;  however  the  solution  is 
proven  feasible  and  the  extremal  nature  is  verified. 


X-Axis 

Figure  15:  Validation  of  solution  optimality  with  Bellman’s  principle. 
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Path  Length 

Figure  16:  Validation  of  optimal  cost  with  Bellman’s  principle. 


4.2  Extended  Applications 

Having  shown  the  validity  of  this  solution  approach,  this  subsection  focuses  on  the  illustration  of  its 
practicality  and  applicability  by  implementing  this  approach  with  various  problems. 

4.21  Various  Obstacle  Shapes 

Section  3.2  introduced  the  concepts  used  to  mathematically  represent  the  various  obstacle  shapes  that  can  be 
incorporated  into  the  optimal  control  problem.  Those  concepts  are  incorporated  below  to  evidence  their 
applicability.  Figure  17  shows  a  time-optimal  trajectory  generated  against  a  multi-shape  obstacle 
environment,  including  squares,  diamonds,  rectangles,  circles,  and  ellipses.  The  result  displays  similar 
characteristics  to  the  three-circle  problem;  specifically,  the  vehicle  travels  in  straight  lines  except  under  the 
situation  of  touching  an  obstacle. 
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Figure  17:  Time-optimal  trajectory  versus  multi-shape  obstacle  environment. 
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Figure  18  illustrates  the  capability  of  this  technique  against  a  polygonal  obstacle  shape.  In  this  particular 
instance,  a  random,  five-sided  obstacle  is  incorporated. 


Figure  18:  Time-optimal  trajectory  versus  polygonal  shape  obstacle. 


4.22  Obstacle  Rich  Environment 

In  the  previous  examples,  the  usefulness  of  optimal  control  techniques  versus  various  obstacle  shapes  was 
evidenced,  but  relatively  few  obstacles  were  incorporated  into  each  environment.  Figure  19  evidences 
optimal  control  techniques  against  a  32-obstacle  environment  with  circular  obstacles. 
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Figure  19:  Time-optimal  trajectory  versus  obstacle  rich  environment. 
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4.24  Control  Failure 

The  previous  examples  show  that  optimal  control  techniques  are  successful  in  various,  complex  environments, 
but  the  problem  in  this  subsection  tests  the  ability  to  find  a  solution  in  the  event  of  UGV  damage.  Figure  24 
shows  the  optimal  control  trajectory  when  the  vehicle  must  turn  left,  i.e.  the  minimum  and  maximum  steering 
angle  values  are  positive.  In  this  situation,  the  solution  is  not  initially  intuitive  or  well  understood.  The  time- 
optimal  result  requires  two  types  of  motion.  The  first  type  combines  forward  motion  of  the  vehicle  with  a 
minimum  steering  angle;  this  results  in  a  maximum  turning  radius.  The  second  type  of  motion  combines  the 
backward  motion  of  the  vehicle  with  a  maximum  steering  angle;  this  results  in  a  minimum  turning  radius.  In 
total,  the  UGV  moves  forward  with  a  minimum  turning  radius  and  then  backward  with  the  maximum  turning 
radius  to  align  for  the  next  forward  motion.  This  type  of  motion  is  used  in  a  repetitive  fashion  in  Figure  24  to 
successfully  navigate  the  obstacles. 


Figure  24:  Time-optimal  maneuver  where  the  vehicle  suffers  a  control  failure. 


4.25  Dynamic  Environments 

Many  real-world  problems  involve  dynamic  environments,  where  either  the  destination  or  obstacles  are 
moving,  and  examples  of  this  would  be  typical  highway  travel  and  a  vehicle  interceptor  mission,  respectively. 
These  problems  require  significant  knowledge  about  the  obstacles  and  targets  in  their  environment,  but  with 
good  sensors  and  positional  knowledge,  these  assumptions  are  not  unrealistic.  For  example,  in  the  case  of  an 
aircraft  carrier  landing,  the  airplane  could  easily  know  accurate  locations  of  itself  and  the  target.  Figure  25 
shows  the  problem  of  a  moving  destination  or  target  with  two  circular  obstacles  along  the  path.  The  results 
are  displayed  as  an  image  generated  from  a  strobe  light  so  that  the  entire  maneuver  is  visible  within  one 
frame.  Throughout  the  trajectory,  the  vehicle  does  not  drive  towards  the  instantaneous  location  of  the  target; 
it  drives  towards  the  final  location  of  the  target.  This  allows  the  trajectory  to  be  optimal. 

The  second  experiment  involves  two,  circular  obstacles  moving  at  different  speeds  and  a  stationary 
destination.  Figure  26  displays  these  results.  In  this  situation,  a  strobe  image  is  confusing;  therefore,  the 
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results  are  displayed  as  movie  frame  scenes.  From  Figure  26  it  is  visible  that  the  vehicle  reaches  the 
destination  without  hitting  the  obstacles;  however,  the  vehicle’s  trajectory  is  within  close  proximity  of  the 
obstacles,  a  visual  indication  of  optimality. 


Figure  25:  Time-optimal  maneuver  toward  a  moving  target  (strobe  image). 
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Figure  26:  Scenes  from  a  time-optimal  maneuver  around  moving  obstacles. 
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